/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.team1160.breakaway.drive;

import com.team1160.breakaway.api.Constants;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.RobotDrive;

/**
 *
 * @author Team 1160 Firebird Robotics 
 */


public class Drive {
    //Static Instance Fields
    ////////////////////////////////////////////////////////////////////////////

    protected RobotDrive drive = new RobotDrive(Constants.C_LEFT_DRIVE_FRONT,
                                                Constants.C_LEFT_DRIVE_BACK,
                                                Constants.C_RIGHT_DRIVE_FRONT,
                                                Constants.C_RIGHT_DRIVE_BACK);
    //assuming two drivetrain motors
    protected Jaguar m_left_front, m_left_back;
    protected Jaguar m_right_front, m_right_back;
    //Encoders
    

    //State Variables
    protected double ftPerSec;
    protected double move, turn;

    //constructors
    ////////////////////////////////////////////////////////////////////////////
    public Drive(){
        this.ftPerSec = 0;

        initMotors();
    }

    //Private Methods/Init

    private void initMotors(){
        //Motors Init
        this.m_left_back = new Jaguar(Constants.C_LEFT_DRIVE_BACK);
        this.m_left_front = new Jaguar(Constants.C_LEFT_DRIVE_FRONT);

        this.m_right_back = new Jaguar(Constants.C_RIGHT_DRIVE_BACK);
        this.m_right_front = new Jaguar(Constants.C_RIGHT_DRIVE_FRONT);

        //Set Bounds
        this.m_left_back.setBounds(Constants.J_MAX_LIMIT,
                Constants.J_DEADBAND_TOP,
                Constants.J_CENTER,
                Constants.J_DEADBAND_BOT,
                Constants.J_MIN_LIMIT);
        this.m_left_front.setBounds(Constants.J_MAX_LIMIT,
                Constants.J_DEADBAND_TOP,
                Constants.J_CENTER,
                Constants.J_DEADBAND_BOT,
                Constants.J_MIN_LIMIT);
        this.m_right_back.setBounds(Constants.J_MAX_LIMIT,
                Constants.J_DEADBAND_TOP,
                Constants.J_CENTER,
                Constants.J_DEADBAND_BOT,
                Constants.J_MIN_LIMIT);
        this.m_right_front.setBounds(Constants.J_MAX_LIMIT,
                Constants.J_DEADBAND_TOP,
                Constants.J_CENTER,
                Constants.J_DEADBAND_BOT,
                Constants.J_MIN_LIMIT);
    }

    //Accessor methods
    ////////////////////////////////////////////////////////////////////////////
    /**
     * Gets the current speed of the robot in ftPerSec.
     *
     * @return Current ftPerSec
     */
    public double getSpeed(){
        return this.ftPerSec;
    }

    public double getMove(){
        return this.move;
    }

    public double getTurn(){
        return this.turn;
    }

    //action methods
    ////////////////////////////////////////////////////////////////////////////
    /**
     * This is a wrapper function for RobotDrive.tankDrive()
     * @param leftSpeed speed of the left motor
     * @param rightSpeed speed of the right motor
     */
    //TODO write arcadeDrivel logic
    public void drive(double move, double rotate) throws IllegalArgumentException{
        move=limit(move);
        turn=limit(turn);
        this.ftPerSec=0;
        this.move=move;
        this.turn=rotate;
        drive.arcadeDrive(move, rotate);

    }

    /**
     * sets the motor with motorID to value
     *
     * @param motorID
     * @param value value from 1-255
     */



    public void calcSpeed(){
        //TODO add calculation of speed here
    }
    //Testing Functions
    ////////////////////////////////////////////////////////////////////////////
    protected double limit(double i){
        if(i<-1.00)
            return -1.00;
        if(i>1.00)
            return 1.00;
        return i;
    }
}
